#include "force_control_cmd.h"
#include "motion/mot_priv.h"
#include "log/zuclog.h"
//#include "tp/tpmanager.hh"
#include "errcode.h"
#include "motion/semintf.h"
#include "dyna/jaka_dyn_intf.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "dyna/jaka_dyn_intf.h"
#include "kine/kine.h"
#include "axisgroup/robot_mgr.h"

#define MOTCMD_INFO(fmt, ...) zuclog_info("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("MOTCMD", fmt, ##__VA_ARGS__)

int CmdSetAdmittanceEna::process()
{
    MOTCMD_INFO("ZUCMOT_SET_ADMITTANCE_ENA\n");
    auto grp = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto dyna = grp->dyna();
    // checkout torqsensor mode
    if (enable_ == 1 && zucmotSensorStruct->sensorStatus.status == 0)
    {
        reportError(ADMIT_CANNOT_ENABLE_WITHOUT_SENSOR, ("Cannot enable admittance control since sensor is not ready"));
    }
    else
    {
        double cartPos[CART_DIM] = {0};
        double Bs[CART_DIM] = {0};
        cartPos[0] = zucmotStatus->carte_pos_cmd.tran.x;
        cartPos[1] = zucmotStatus->carte_pos_cmd.tran.y;
        cartPos[2] = zucmotStatus->carte_pos_cmd.tran.z;
        cartPos[3] = zucmotStatus->carte_pos_cmd.a * PM_PI / 180.0;
        cartPos[4] = zucmotStatus->carte_pos_cmd.b * PM_PI / 180.0;
        cartPos[5] = zucmotStatus->carte_pos_cmd.c * PM_PI / 180.0;

        if (zucmotSensorStruct->sensorConfig.torqsensorBrand == 4)
        {
            dyna->admittanceInitBFT(cartPos, Bs, zucmotStatus->toolPayLoad.m, zucmotStatus->toolPayLoad.p);
        }
        else
        {
            dyna->admittanceSynpos(cartPos);
            dyna->resetAdmittanceVel();
            if (zucmotStatus->compliantEnable)
            {
                int sem_id = sem_get(SENSOR_SEM_KEY);
                double torque[CART_DIM] = {0};
                if (sem_p(sem_id) == 0)
                {
                    for (int joint_num = 0; joint_num < CART_DIM; joint_num++) { torque[joint_num] = zucmotSensorStruct->sensorStatus.torque[joint_num]; }
                    sem_v(sem_id);
                }
                // 计算传感器中心位姿	@FIXME
                zucmot_joint_t* joint = NULL;
                double joint_cmd[ZUCMOT_MAX_JOINTS] = {0};
                for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
                {
                    joint = &joints[joint_num];
                    //joint_fb[joint_num] = joint->pos_fb;
                    joint_cmd[joint_num] = joint->pos_cmd;
                }
                ZucPose sensor_CartPos = {0};
                ZucPose tool_offset = {0};  // @FIXME  当前默认传感器安装与末端法兰一致
                auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine();
                kine->fkine(RobJointVal(joint_cmd), sensor_CartPos, &tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);
                double s_pos[CART_DIM] = {0};
                s_pos[0] = sensor_CartPos.tran.x;
                s_pos[1] = sensor_CartPos.tran.y;
                s_pos[2] = sensor_CartPos.tran.z;
                s_pos[3] = sensor_CartPos.a * PM_PI / 180.0;
                s_pos[4] = sensor_CartPos.b * PM_PI / 180.0;
                s_pos[5] = sensor_CartPos.c * PM_PI / 180.0;
                // MOTCMD_INFO("ZUCMOT_SET_ADMITTANCE_ENA : %lf, %lf, %lf, %lf, %lf, %lf\n", s_pos[0], s_pos[1], s_pos[2], s_pos[3], s_pos[4], s_pos[5]);

                dyna->admittanceInit(s_pos, torque, zucmotStatus->toolPayLoad.m, zucmotStatus->toolPayLoad.p);
            }
        }
        zucmotStatus->admittance_enabled = enable_;
    }
    return 0;
}

int CmdSetCompliantType::process()
{
    MOTCMD_INFO("ZUCMOT_SET_COMPLIANT_TYPE\n");
    auto grp = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto dyna = grp->dyna();
    // checkout torqsensor mode
    if (enable_ == 1 && zucmotSensorStruct->sensorStatus.status == 0)
    {
        reportError(ADMIT_CANNOT_ENABLE_WITHOUT_SENSOR, ("Cannot enable admittance control since sensor is not ready"));
    }
    else
    {
        double cartPos[CART_DIM] = {0};
        double Bs[CART_DIM] = {0.0};
        cartPos[0] = zucmotStatus->carte_pos_cmd.tran.x;
        cartPos[1] = zucmotStatus->carte_pos_cmd.tran.y;
        cartPos[2] = zucmotStatus->carte_pos_cmd.tran.z;
        cartPos[3] = zucmotStatus->carte_pos_cmd.a * PM_PI / 180.0;
        cartPos[4] = zucmotStatus->carte_pos_cmd.b * PM_PI / 180.0;
        cartPos[5] = zucmotStatus->carte_pos_cmd.c * PM_PI / 180.0;
        bool type_changed = true;
        if (zucmotStatus->compliantType == compliantType_)
        {
            type_changed = false;
        }
        zucmotStatus->compliantType = compliantType_;
        zucmotStatus->compliantEnable = compliantEnable_;
        if (zucmotStatus->compliantType == 0)
        {
            double joint_pos_cmd[ZUCMOT_MAX_JOINTS] = {0.0};
            for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
            {
                zucmot_joint_t* joint = &joints[joint_num];
                joint_pos_cmd[joint_num] = joint->motor_pos_cmd;
            }
            //            tpManager_tpSetPos_JPos(&zucmotStatus->carte_pos_cmd, joint_pos_cmd);
        }
        MOTCMD_INFO("zucmotStatus->compliantEnable = %d\n", zucmotStatus->compliantEnable);
        if (zucmotSensorStruct->sensorConfig.torqsensorBrand == 4)
        {
            dyna->admittanceInitBFT(cartPos, Bs, zucmotStatus->toolPayLoad.m, zucmotStatus->toolPayLoad.p);
        }
        else
        {
            if (type_changed)
            {
                dyna->admittanceSynpos(cartPos);
                dyna->resetAdmittanceVel();
            }
            if (zucmotStatus->compliantEnable)
            {
                int sem_id = sem_get(SENSOR_SEM_KEY);
                double torque[CART_DIM] = {0};
                if (sem_p(sem_id) == 0)
                {
                    for (int joint_num = 0; joint_num < CART_DIM; joint_num++) { torque[joint_num] = zucmotSensorStruct->sensorStatus.torque[joint_num]; }
                    sem_v(sem_id);
                }
                // 计算传感器中心位姿	@FIXME
                zucmot_joint_t* joint = NULL;
                double joint_fb[ZUCMOT_MAX_JOINTS] = {0};
                double joint_cmd[ZUCMOT_MAX_JOINTS] = {0};
                for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
                {
                    joint = &joints[joint_num];
                    joint_fb[joint_num] = joint->pos_fb;
                    joint_cmd[joint_num] = joint->pos_cmd;
                }
                ZucPose sensor_CartPos = {0};
                ZucPose tool_offset = {0};  // @FIXME  当前默认传感器安装与末端法兰一致
                auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine();
                kine->fkine(RobJointVal(joint_fb), sensor_CartPos, &tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);
                double s_pos[CART_DIM] = {0};
                s_pos[0] = sensor_CartPos.tran.x;
                s_pos[1] = sensor_CartPos.tran.y;
                s_pos[2] = sensor_CartPos.tran.z;
                s_pos[3] = sensor_CartPos.a * PM_PI / 180.0;
                s_pos[4] = sensor_CartPos.b * PM_PI / 180.0;
                s_pos[5] = sensor_CartPos.c * PM_PI / 180.0;

                // MOTCMD_INFO("ZUCMOT_SET_COMPLIANT_TYPE : %lf, %lf, %lf, %lf, %lf, %lf\n", s_pos[0], s_pos[1], s_pos[2], s_pos[3], s_pos[4], s_pos[5]);
                dyna->admittanceInit(s_pos, torque, zucmotStatus->toolPayLoad.m, zucmotStatus->toolPayLoad.p);
            }
        }
        //        // tpManager_tpSetPos_JPos( &zucmotStatus->carte_pos_cmd, joint_pos_cmd);
        // MOTCMD_INFO("zucmotStatus->compliantEnable:%d\n",zucmotStatus->compliantEnable);
        // MOTCMD_INFO("zucmotStatus->compliantType:%d\n",zucmotStatus->compliantType);
    }
    return 0;
}

int CmdSetAdmittanceOpt::process()
{
    for (int n = 0; n < CART_DIM; n++)
    {
        zucmotStatus->admit_axis_opt[n] = admit_axis_opt_[n];
        MOTCMD_INFO("admit_axis_opt[%d] = %d\n", n, admit_axis_opt_[n]);
    }
    return 0;
}

int CmdSetAdmittanceConfig::process()
{
    // if (zucmotStatus->compliantType || GET_MOTION_ADMITTANCE_FLAG())
    // {
    //     if (enableFt_ == 0 && zucmotStatus->ftConfig[id_].enableFt)
    //     {
    //         reportError(ERR_REDUCE_ADMITTANCE_WHEN_FC_ON, ("It's not allowed to reduce force control directions while force control is still on."));
    //         return -1;
    //     }
    // }
    // zucmotStatus->ftConfig[dir_] = ftConfig_;
    return 0;
}

int CmdSetVelCompliantCtrl::process()
{
    for (int i = 0; i < 5; i++)
    {
        zucmotStatus->velCompliantCtrl[i] = velCompliantCtrl_[i];
        MOTCMD_INFO("zucmotStatus->velCompliantCtrl[%d] = %f\n", i, velCompliantCtrl_[i]);
    }
    return 0;
}

int CmdSetAdmittanceCtrlFrame::process()
{
    zucmotStatus->ftFrame = ftFrame_;
    MOTCMD_INFO("zucmotStatus->ftFrame  = %d\n", zucmotStatus->ftFrame);
    return 0;
}

int CmdDisableForceControl::process()
{
    if (zucmotStatus->compliantType != 0)
        zucmotStatus->disableForceCtrl = disableForceCtrl_;
    else
        zucmotStatus->disableForceCtrl = 0;
    double joint_pos_cmd[ZUCMOT_MAX_JOINTS] = {0.0};
    double cartPos1[CART_DIM] = {0.0};
    for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        zucmot_joint_t* joint = &joints[joint_num];
        // joint_pos_fb[joint_num] = joint->motor_pos_fb;
        joint_pos_cmd[joint_num] = joint->motor_pos_cmd;
    }
    //    tpManager_tpSetPos_JPos(&zucmotStatus->carte_pos_cmd, joint_pos_cmd);
    cartPos1[0] = zucmotStatus->carte_pos_cmd.tran.x;
    cartPos1[1] = zucmotStatus->carte_pos_cmd.tran.y;
    cartPos1[2] = zucmotStatus->carte_pos_cmd.tran.z;
    cartPos1[3] = zucmotStatus->carte_pos_cmd.a * PM_PI / 180.0;
    cartPos1[4] = zucmotStatus->carte_pos_cmd.b * PM_PI / 180.0;
    cartPos1[5] = zucmotStatus->carte_pos_cmd.c * PM_PI / 180.0;
    auto grp = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto dyna = grp->dyna();
    dyna->admittanceSynpos(cartPos1);
    dyna->resetAdmittanceVel();
    MOTCMD_INFO("zucmotStatus->disableForceCtrl  = %d\n", zucmotStatus->disableForceCtrl);
    return 0;
}

int CmdSetFtCompliantCondition::process()
{
    for (int i = 0; i < CART_DIM; i++)
    {
        zucmotStatus->compliantCondition[i] = compliantCondition_[i];
        MOTCMD_INFO("zucmotStatus->compliantCondition[%d] = %f\n", i, compliantCondition_[i]);
    }
    return 0;
}

int CmdSetpayloadTool::process()
{
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        zucmotStatus->toolPayLoad = payload_;
        MOTCMD_INFO("sensor payload: %f, p: %f, %f, %f\n", payload_.m, payload_.p[0], payload_.p[1], payload_.p[2]);
        sem_v(sem_id);
    }
    return 0;
}

int CmdSettorqueSensorSoftLimit::process()
{
    for (int n = 0; n < CART_DIM; n++)
    {
        zucmotStatus->torqueSensorSoftLimit[n] = torqueSensorSoftLimit_[n];
        MOTCMD_INFO("torqueSensorSoftLimit[%d] = %f\n", n, torqueSensorSoftLimit_[n]);
    }
    return 0;
}

int CmdSetadmittanceDragDeadZone::process()
{
    for (int n = 0; n < CART_DIM; n++) { zucmotStatus->drag_dead_zone[n] = admittanceDragDeadZone_[n]; }
    auto grp = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto dyna = grp->dyna();
    dyna->SetDragDeadZone(zucmotStatus->drag_dead_zone);
    MOTCMD_INFO("AdmittanceDragDeadZone = %f, %lf, %lf, %lf, %lf, %lf\n",
                zucmotStatus->drag_dead_zone[0],
                zucmotStatus->drag_dead_zone[1],
                zucmotStatus->drag_dead_zone[2],
                zucmotStatus->drag_dead_zone[3],
                zucmotStatus->drag_dead_zone[4],
                zucmotStatus->drag_dead_zone[5]);
    return 0;
}

int CmdSettorqueSensorFilter::process()
{
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        zucmotSensorStruct->torqueSensorFilter = torqueSensorFilter_;
        sem_v(sem_id);
    }
    return 0;
}
